#include "EnemyFlagStolenState.h"
#include "World.h"
#include "470bot.h"
#include "Vector2.h"
#include "KalmanTank.h"
#include "TargetingHelper.h"
#include "TimeHelper.h"
#include "PDController.h"

EnemyFlagStolenState::EnemyFlagStolenState(void)
{
}


EnemyFlagStolenState::~EnemyFlagStolenState(void)
{
}

Command EnemyFlagStolenState::GetCommand(Agent* agent) {
	World* world = World::instance();
	tank_t* tank = agent->getTank();
	Vector2 tank_pos(tank->pos[0], tank->pos[1]);

	double min_dist = 99999999;
	KalmanTank* target = 0;

	for (unsigned int i = 0; i < world->enemies().size(); i++) {
		KalmanTank* ktank = &world->enemies().at(i);
		otank_t* otank = ktank->GetStruct();
		if (otank->status != "alive")
			continue;

		double dist = (ktank->GetCurrentEstimate() - tank_pos).magnitude();

		/*if (dist < min_dist) {
			min_dist = dist;
			target = ktank;
		}*/

		if(dist <= World::instance()->getShotRange()){
			if(target == 0){
				min_dist = dist;
				target = ktank;
			}
			else if(min_dist > dist){
				min_dist = dist;
				target = ktank;
			}
		}
	}

	if (target) {

		Vector2 target_pos = target->GetCurrentEstimate();
		
		Command cmd = TargetingHelper::Target(tank, target);
		
		cmd.speed(1);
		
		return cmd;
	}
	else{
		tank_t* flagBearer = World::instance()->flagBearer();
		Vector2 trajectory(flagBearer->pos[0] - agent->getTank()->pos[0], flagBearer->pos[1] - agent->getTank()->pos[1]);

		if(trajectory.magnitude() < FLAG_BEARER_THRESHOLD){
			trajectory = World::instance()->flagBearerDir();
		}

		Command cmd = PDController::instance()->GetCommand(agent->getTank(), trajectory);
		cmd.shoot(true);
		return cmd;
	}

	return Command(agent->getTank()->index, 0, 0, 0);
}